﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using grounding.nao;
using grounding.parser;
using grounding.grounder;
using System.Speech.Recognition;
using grounding.parser.Parsers;
using grounding.nao.enums;

namespace grounding.tests.GroundingDemo
{
    class Program
    {
        private const string NAO_IP = "128.208.4.19"; // loopback ip: 127.0.0.1, current nao ip: 128.208.4.225
        //private const string NAO_IP = "127.0.0.1";
        private const int NAO_PORT = 9559;

        public static Nao nao;
        private static SpeechToText speechToText;
        private static DemoActionParser parser;
        private static DemoActionTemplate template = null;
        private static List<string> needsClarification = new List<string>();
        private static bool hasAskedForClarification = false;

        public static void Main(string[] args)
        {
            InitializeComponents();

            //nao.StiffAll();

            //nao.ExtendArms();

            //nao.UnstiffAll();

            //nao.PerformGesture("Touchdown");

            //nao.Walk(0.5f, Direction.Backward);
            
            //Pose p = nao.getCurrentPose();

            //nao.BatteryReport(true);

            // start recognizing things!
            speechToText.Start();

            // Keep the console open. Will exit when any key is hit.
            Console.ReadKey(true);

            // Unstiff motors and disable sonar before exit.
            nao.Disconnect();
        }

        /// <summary>
        /// Prepare everything for execution.
        /// </summary>
        public static void InitializeComponents()
        {
            nao = new Nao(NAO_IP, NAO_PORT);
            speechToText = new SpeechToText();
            speechToText.DebugMode = false;
            speechToText.SpeechRecognized += new EventHandler<System.Speech.Recognition.SpeechRecognizedEventArgs>(speechToText_SpeechRecognized);
            parser = new DemoActionParser();
        }

        static void speechToText_SpeechRecognized(object sender, System.Speech.Recognition.SpeechRecognizedEventArgs e)
        {
            Console.WriteLine("I heard you say: {0}", e.Result.Text);

            if (needsClarification.Count == 0)
            {
                template = (DemoActionTemplate)parser.Parse(e.Result.Text);
                if (template == null)
                    Console.WriteLine("Ignored phrase");
                else
                {
                    Console.WriteLine("I understood it as:");

                    foreach (string option in template.info.Keys)
                    {
                        if(template.info[option] == null)
                            needsClarification.Add(option);
                        Console.WriteLine("\t" + option + ": " + template.info[option]);
                    }

                    if (needsClarification.Count == 0)
                    {
                        makeMove(template);
                        hasAskedForClarification = false;
                    }
                    else
                    {
                        foreach (string option in needsClarification)
                        {
                            if (!hasAskedForClarification)
                                nao.Say("Please clarify " + option);
                            hasAskedForClarification = true;
                            Console.WriteLine("You need to clarify " + option);
                        }
                    }
                }
            }
            else
            {
                template.FillTemplate(e.Result.Text, true);
                needsClarification.Clear();
                foreach (string option in template.info.Keys)
                {
                    if (template.info[option] == null)
                        needsClarification.Add(option);
                }

                if (needsClarification.Count == 0)
                {
                    makeMove(template);
                    hasAskedForClarification = false;
                }
                else
                {
                    foreach (string option in needsClarification)
                    {
                        if (!hasAskedForClarification)
                            nao.Say("Please clarify " + option);
                        hasAskedForClarification = true;
                        Console.WriteLine("You need to clarify " + option);
                    }
                }
            }
        }

        static void makeMove(DemoActionTemplate fullTemplate)
        {
            nao.StopTalking();
            if (fullTemplate.Type == "walking")
            {
                float distance;
                Direction direction;
                try
                {
                    //distance = float.Parse(fullTemplate.info["distance"]);
                    switch (fullTemplate.info["distance"])
                    {
                        case "one":
                            distance = 1;
                            break;
                        case "two":
                            distance = 2;
                            break;
                        case "three":
                            distance = 3;
                            break;
                        default:
                            throw new ArgumentException("YOU DIDN'T PUT A NUMBER");
                    }
                    direction = (Direction)Enum.Parse(typeof(Direction), fullTemplate.info["direction"], true);
                }
                catch (Exception ex)
                {
                    Console.WriteLine("Failed to parse filled in template values");
                    Console.WriteLine(ex.StackTrace);
                    return;
                }

                nao.Walk(distance, direction);
            }
            else if (fullTemplate.Type == "waving")
            {
                int repetitions;
                //string side;
                try
                {
                    //repetitions = int.Parse(fullTemplate.info["repetitions"]);
                    switch (fullTemplate.info["repetitions"])
                    {
                        case "once":
                        case "one time":
                        case "one times":
                            repetitions = 1;
                            break;
                        case "twice":
                        case "two time":
                        case "two times":
                            repetitions = 2;
                            break;
                        default:
                            throw new ArgumentException("Unable to parse repetitions");
                    }
                    //side = fullTemplate.info["side"];
                }
                catch (Exception ex)
                {
                    Console.WriteLine("Failed to parse filled in template values");
                    Console.WriteLine(ex.StackTrace);
                    return;
                }

                for (int i = 0; i < repetitions; i++)
                    nao.PerformGesture("WaveGoodbye");
            }
        }
    }
}
